#pragma once

#include "PointInfo.h"
#include "boost/multi_array.hpp"
#include "btBulletCollisionCommon.h"
#include <iostream>

/// Odpowiada za tworzenie i redukcje grafow.
class GraphManager
{
public:
	/** Redukuje liczbe wierzcholkow
	* @param input Lista wierzcholkow do zredukowania
	* @return Zredukowana lista wierzcholkow.
	*/
	static inline btAlignedObjectArray<btVector3> Reduce(btAlignedObjectArray<btVector3>& input)
	{
		int n = input.size();
		btAlignedObjectArray<btVector3> result;
		
		if(n > 0)
		{
			btAlignedObjectArray<int> removedIndexes;

			for(int i = 0; i < n; ++i)
			{
				for(int j = i + 1; j < n; ++j)
				{
					for(int k = j + 1; k < n; ++k)
					{
						if(!PointInfo::AreColinear(input[i], input[j], input[k]))
							continue;

						bool removed = false;
							if(!removed)
							{
								btVector3* v12 = new btVector3(input[j] - input[i]);
								btVector3* v13 = new btVector3(input[k] - input[i] );
								btScalar angle = abs(v12->angle(*v13));
								/*std::cout << "\t 12 vs 13: " << angle << std::endl;*/
								if(angle > 0)
								{
									removed = true;
									if(removedIndexes.findLinearSearch(i) == removedIndexes.size())
										removedIndexes.push_back(i);
								}

								delete v12;
								delete v13;
							}

							if(!removed)
							{
								btVector3* v21 = new btVector3(input[i] - input[j]);
								btVector3* v23 = new btVector3(input[k] - input[j]);
								btScalar angle = abs(v21->angle(*v23));
								//std::cout << "\t 21 vs 23: " << angle << std::endl;
								if(angle > 0)
								{
									removed = true;
									if(removedIndexes.findLinearSearch(j) == removedIndexes.size())
										removedIndexes.push_back(j);
								}

								delete v21;
								delete v23;
							}

							if(!removed)
							{

								btVector3* v31 = new btVector3(input[i] - input[k]);
								btVector3* v32 = new btVector3(input[j] - input[k]);
								btScalar angle = abs(v31->angle(*v32));
								//std::cout << "\t 31 vs 32: " << angle << std::endl;
								if(angle > 0)
								{
									removed = true;
									if(removedIndexes.findLinearSearch(k) == removedIndexes.size())
										removedIndexes.push_back(k);
								}

								delete v31;
								delete v32;
							}			
					}
				}
			}

			for(int i = 0; i < n; ++i)
			{
				if(removedIndexes.findBinarySearch(i) == removedIndexes.size())
					result.push_back(input[i]);
			}
		}	

		return result;
	}

	/** Redukuje liczbe wierzcholkow z danej sceny
	* @param world Scena.
	* @param input Lista wierzcholkow do zredukowania
	* @return Zredukowana lista wierzcholkow.
	*/
	static inline btAlignedObjectArray<PointInfo> Reduce(btCollisionWorld* world, btAlignedObjectArray<PointInfo>& input)
	{
		btAlignedObjectArray<PointInfo> result;
		int n = input.size();
		std::cout << "Do zredukowania " << n << std::endl;
		try
		{
			if(n > 0)
			{
				std::cout << "Tworze graf " << std::endl;
				boost::multi_array<float, 2>& graph = CreateGraph(input, world);
				std::cout << "Graf o rozmiarach " << graph.shape()[0] << std::endl;
				
				//for(int i = 0; i < n; ++i)
				//{
				//	for(int j = 0; j < n; ++j)
				//	{
				//		std::cout << "\t" << graph[i][j];
				//	}
				//	std::cout << std::endl;
				//}
				//std::cout << "================" << std::endl;
#pragma region Checking collinearity
				for(int i = 0; i < n; ++i)
				{
					PointInfo& p1 = input[i];
					for(int j = i + 1; j < n; ++j)
					{
						PointInfo& p2 = input[j];
						for(int k = j + 1; k < n; ++k)
						{
							if(graph[i][j] == 0 || graph[j][k] == 0 || graph[i][k] == 0)
								continue;

							PointInfo& p3 = input[k];



							if(!PointInfo::AreColinear(p1, p2, p3))
								continue;

							btVector3* v1 = new btVector3(p1.X, p1.Y, p1.Z);
							btVector3* v2 = new btVector3(p2.X, p2.Y, p2.Z);
							btVector3* v3 = new btVector3(p3.X, p3.Y, p3.Z);

							bool removed = false;
							if(!removed)
							{
								btVector3* v12 = new btVector3((*v2 - *v1));
								btVector3* v13 = new btVector3((*v3 - *v1));
								btScalar angle = abs(v12->angle(*v13));
								/*std::cout << "\t 12 vs 13: " << angle << std::endl;*/
								if(angle > 0)
								{
									removed = true;
									p1.Removed = true;								
								}

								delete v12;
								delete v13;
							}

							if(!removed)
							{
								btVector3* v21 = new btVector3((*v1 - *v2));
								btVector3* v23 = new btVector3((*v3 - *v2));
								btScalar angle = abs(v21->angle(*v23));
								//std::cout << "\t 21 vs 23: " << angle << std::endl;
								if(angle > 0)
								{
									removed = true;
									p2.Removed = true;								
								}

								delete v21;
								delete v23;
							}

							if(!removed)
							{

								btVector3* v31 = new btVector3((*v1 - *v3));
								btVector3* v32 = new btVector3((*v2 - *v3));
								btScalar angle = abs(v31->angle(*v32));
								//std::cout << "\t 31 vs 32: " << angle << std::endl;
								if(angle > 0)
								{
									removed = true;
									p3.Removed = true;
								}

								delete v31;
								delete v32;
							}						

							delete v1;
							delete v2;
							delete v3;
						}					
					}
				}
#pragma endregion

				for(int i = 0; i < n; ++i)
				{
					//std::cout << input[i].ToString() << " " << input[i].Removed << std::endl;
					if(!input[i].Removed)
						result.push_back(input[i]);
				}
			}
		}
		catch(std::string e)
		{
			std::cout << e << std::endl;
		}

		std::cout << "Po redukcji: " << result.size() << std::endl;
		for(int i = 0; i < result.size(); ++i)
		{
			std::cout << "\t";
			std::cout << result[i].ToString();
			std::cout << std::endl;
		}
		return result;
	}

	/** Tworzy graf na podstawie listy punktow
	* @param input Lista punktow swiata.
	* @param world Wskaznik na klase swiata.
	* @return Utworzony graf.
	*/
	static inline boost::multi_array<float, 2> CreateGraph(btAlignedObjectArray<btVector3>& input, btCollisionWorld* world)
	{
		int n = input.size();
		if(n == 0)
			throw "Cannot create graph from empty list.";
		boost::multi_array<float, 2> tmp(boost::extents[n][n]);

		for(int i = 0; i < n; ++i)
		{
			for(int j = i + 1; j < n; ++j)
			{
				btCollisionWorld::ClosestRayResultCallback callback(input[i], input[j]);
				world->rayTest(input[i], input[j], callback);
				if(!callback.hasHit())
				{
					tmp[i][j] = tmp[j][i] = input[i].distance(input[j]);					
				}
			}
		}

		return tmp;
	}

	/** Tworzy graf na podstawie listy punktow
	* @param input Lista punktow swiata.
	* @param world Wskaznik na klase swiata.
	* @return Utworzony graf.
	*/
	static inline boost::multi_array<float, 2> CreateGraph(btAlignedObjectArray<PointInfo>& input, btCollisionWorld* world)
	{
		int n = input.size();
		if(n == 0)
			throw "Cannot create graph from empty list.";

		boost::multi_array<float, 2> tmp(boost::extents[n][n]);

		std::cout << "Buduje graf " << n << std::endl;
		for(int i = 0; i < n; ++i)
		{
			//std::cout << "\t" << input[i].ToString(true) << std::endl;
			btVector3 a(input[i].RealCoords.X, input[i].RealCoords.Y, input[i].RealCoords.Z);
			for(int j = i + 1; j < n; ++j)
			{
				btVector3 b(input[j].RealCoords.X, input[j].RealCoords.Y, input[j].RealCoords.Z);	

				btCollisionWorld::ClosestRayResultCallback callback(a, b);
				world->rayTest(a,b, callback);
				//std::cout << "\n\t" << callback.hasHit();
				if(!callback.hasHit())
				{
					tmp[i][j] = tmp[j][i] = a.distance(b);
					//std::cout << "\t" << tmp[i][j] << std::endl;
				}

			}			
		}

		return tmp;
	}
};


